#include "drone_listener.h"

const char SPLIT_CHAR = ' ';

DroneListener::DroneListener()
    : nh()
{
    drone_state_sub = nh.subscribe<mavros_msgs::State>(MAVROS_INFO_STATE, GLOBAL_DEFAULT_QUEUE_SIZE,
                                                       std::bind(&DroneListener::StateCallback, this, std::placeholders::_1));
    drone_current_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>(MAVROS_INFO_LOCAL_POSE, GLOBAL_DEFAULT_QUEUE_SIZE,
                                                                     std::bind(&DroneListener::PoseCallback, this, std::placeholders::_1));
    drone_current_gpspos_sub = nh.subscribe<sensor_msgs::NavSatFix>(MAVROS_INFO_GPS_POS, GLOBAL_DEFAULT_QUEUE_SIZE,
                                                                    std::bind(&DroneListener::GPSCallback, this, std::placeholders::_1));
    drone_current_velocity_sub = nh.subscribe<geometry_msgs::TwistStamped>(MAVROS_INFO_VELOCITY, GLOBAL_DEFAULT_QUEUE_SIZE,
                                                                           std::bind(&DroneListener::VelocityCallback, this, std::placeholders::_1));

    mavproxyStatePub = nh.advertise<mavproxy_ros1::MavproxyState>(MAVPROXY_STATE, GLOBAL_DEFAULT_QUEUE_SIZE);
}

void DroneListener::StateCallback(const mavros_msgs::State::ConstPtr &msg)
{
    drone_current_state = *msg;
}

void DroneListener::PoseCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    drone_current_pose = *msg;
}

void DroneListener::VelocityCallback(const geometry_msgs::TwistStamped::ConstPtr &msg)
{
    drone_current_velocity = *msg;
}

void DroneListener::GPSCallback(const sensor_msgs::NavSatFix::ConstPtr &msg)
{
    drone_current_gpose.latitude = (*msg).latitude;
    drone_current_gpose.longitude = (*msg).longitude;
    drone_current_gpose.altitude = (*msg).altitude;
}

// 更新mavproxy获取的无人机状态
void DroneListener::FlushMavproxyState(){
    mavproxy_ros1::MavproxyState temp;

    temp.header.seq = sent_seq++;
    temp.header.stamp = ros::Time::now();
    temp.header.frame_id = "local_origin";

    temp.local_position = drone_current_pose.pose;
    temp.gps_position = drone_current_gpose;
    temp.armed = drone_current_state.armed;
    temp.connected = drone_current_state.connected;
    temp.rc_breakin = drone_current_state.manual_input;
    temp.safe_breakin = false;
    temp.flight_mode = drone_current_state.mode;

    mavproxy_state = temp;
}

void DroneListener::Run(){
    ROS_INFO("Enter to run drone state listener");

    ros::Rate rate(GLOBAL_COMMON_ROS_RATE / 2);

    ros::Time last_request = ros::Time::now();
    while (ros::ok() && !drone_current_state.connected && ros::Time::now() - last_request < ros::Duration(DRONE_CONNECTION_TIMEOUT_DURATION))
    {
        ros::spinOnce();
        rate.sleep();
    }

    while(ros::ok){
        FlushMavproxyState();
        mavproxyStatePub.publish(mavproxy_state);

        ros::spinOnce();
        rate.sleep();
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "drone_listener");
    DroneListener node;
    node.Run();
    ros::spin();
    return RETSUCCESS;
}
